/*
 * particle_filter.cpp
 */
#include "particle_filter.h"
#include <output/output.h>
#include <random>
#include <algorithm>
#include <iostream>
#include <numeric>
#include <fstream>

#ifndef EPS
#define EPS 1e-12
#endif

using namespace std;
using namespace Filtering;

void ParticleFilter::init(double x, double y, double z, double theta, double std[], int num_particles) 
{
	// Set the number of particles. Initialize all particles to first position (based on estimates of
	//   x, y, theta and their uncertainties from GPS) and all weights to 1. 
	// Add random Gaussian noise to each particle.
	// NOTE: Consult particle_filter.h for more information about this method (and others in this file).
    num_particles_ = num_particles; //set to number of files in observation directory

    weights.resize(num_particles_);
    particles.resize(num_particles_);

    double std_x, std_y, std_theta; // Standard deviations for x, y, and theta
    std_x = std[0];
    std_y = std[1];
    std_theta = std[2];

    // Normal distribution for x, y and theta
    normal_distribution<double> dist_x(x, std_x); // mean is centered around the new measurement
    normal_distribution<double> dist_y(y, std_y);
    normal_distribution<double> dist_theta(theta, std_theta);

    default_random_engine gen; //http://www.cplusplus.com/reference/random/default_random_engine/

    // create particles and set their values
    for(int i = 0; i < num_particles_; ++i){
        Particle p;
        p.id = i;
        p.x = dist_x(gen); // take a random value from the Gaussian Normal distribution and update the attribute
        p.y = dist_y(gen);
        p.z = z;
        p.theta = dist_theta(gen);
        p.weight = 1;

        particles[i] = p;
        weights[i] = p.weight;
    }
    is_initialized = true;
}

void ParticleFilter::prediction(double delta_t, double std_pos[], double velocity, double yaw_rate)
{
	// Add measurements to each particle and add random Gaussian noise.
	// NOTE: When adding noise you may find std::normal_distribution and std::default_random_engine useful.
	//  http://en.cppreference.com/w/cpp/numeric/random/normal_distribution
	//  http://www.cplusplus.com/reference/random/default_random_engine/

    double std_x, std_y, std_theta; // Standard deviations for x, y, and theta
    std_x = std_pos[0];
    std_y = std_pos[1];
    std_theta = std_pos[2];

    default_random_engine gen;
    for(int i=0; i < num_particles_; ++i)
    {
        Particle *p = &particles[i]; // get address of particle to update

        double new_x = p->x, new_y = p->y, new_theta = p->theta;
        if(std::fabs(yaw_rate) < EPS)
        {
            new_x = p->x + velocity * delta_t * std::cos(p->theta);
            new_y = p->y + velocity * delta_t * std::sin(p->theta);
            new_theta = p->theta;
        }
        else
        {
            new_x = p->x + (velocity/yaw_rate) * (sin(p->theta + yaw_rate * delta_t) - sin(p->theta));
            new_y = p->y + (velocity/yaw_rate) * (cos(p->theta) - cos(p->theta + yaw_rate * delta_t));
            new_theta = p->theta + (yaw_rate*delta_t);
        }

        // add Gaussian Noise to each measurement
        // Normal distribution for x, y and theta
        normal_distribution<double> dist_x(new_x, std_x);
        normal_distribution<double> dist_y(new_y, std_y);
        normal_distribution<double> dist_theta(new_theta, std_theta);

        // update the particle attributes
        p->x = dist_x(gen);
        p->y = dist_y(gen);
        p->theta = dist_theta(gen);
    }
}

void ParticleFilter::updateWeights(double x, double y, double z, double bearing, double std_landmark[]) 
{
    double std_x = std_landmark[0];
    double std_y = std_landmark[1];
    double std_theta = std_landmark[2];
    double weights_sum = 0;

    Eigen::Matrix2d p2c;
    p2c << std::cos(bearing), -std::sin(bearing),
        std::sin(bearing), std::cos(bearing);

    for(int i = 0; i< num_particles_; ++i)
    {
        Particle *p = &particles[i];
        Eigen::Vector2d local2D = p2c.transpose() * (Eigen::Vector2d(p->x, p->y) - Eigen::Vector2d(x, y));

        double num = std::exp(-0.5 * (std::pow(local2D.x(), 2) / pow(std_x, 2) + std::pow(local2D.y(), 2) / pow(std_y, 2) + std::pow(p->theta - bearing, 2) / pow(std_theta, 2)));
        double denom = 2 * M_PI * std_x * std_y * std_theta;

        double wt = num/denom;
        weights_sum += wt;
        p->weight = wt;
    }

    // normalize weights to bring them in (0, 1]
    for (int i = 0; i < num_particles_; i++)
    {
        Particle *p = &particles[i];
        p->z = z;
        p->weight /= weights_sum;
        weights[i] = p->weight;
    }
}

void ParticleFilter::resample() {
	// Resample particles with replacement with probability proportional to their weight.
	// NOTE: You may find std::discrete_distribution helpful here.
	//   http://en.cppreference.com/w/cpp/numeric/random/discrete_distribution

    default_random_engine gen;

    // Random integers on the [0, n) range
    // the probability of each individual integer is its weight of the divided by the sum of all weights.
    discrete_distribution<int> distribution(weights.begin(), weights.end());
    vector<Particle> resampled_particles;

    for (int i = 0; i < num_particles_; i++){
        Particle particle = particles[distribution(gen)];
        particle.weight = 1.0;
        resampled_particles.emplace_back(std::move(particle));
    }

    particles.swap(resampled_particles);
}

void ParticleFilter::write(std::string filename) {
	// You don't need to modify this file.
	std::ofstream dataFile;
	dataFile.open(filename, std::ios::app);
	for (int i = 0; i < num_particles_; ++i) {
		dataFile << particles[i].x << " " << particles[i].y << " " << particles[i].theta << "\n";
	}
	dataFile.close();
}

bool ParticleFilter::GetCurrentLoc(double& x, double& y, double& z, double& theta)
{
    if(num_particles_ == 0)
        return false;

    double mean_x = 0.0, mean_y = 0.0, mean_z = 0.0, mean_theta = 0.0;
    for(int i = 0; i < num_particles_; i++)
    {
        mean_x += particles[i].x;
        mean_y += particles[i].y;
        mean_z += particles[i].z;
        mean_theta += particles[i].theta;
    }
    mean_x /= num_particles_;
    mean_y /= num_particles_;
    mean_z /= num_particles_;
    mean_theta /= num_particles_;
    x = mean_x;
    y = mean_y;
    z = mean_z;
    theta = mean_theta;
    return true;
}

bool ParticleFilter::GetCurLoc(GeoPoint& gpsPoint)
{
    if(num_particles_ == 0)
        return false;

    double mean_x = 0.0, mean_y = 0.0, mean_z = 0.0, mean_theta = 0.0;
    for(int i = 0; i < num_particles_; i++)
    {
        mean_x += particles[i].x;
        mean_y += particles[i].y;
        mean_z += particles[i].z;
        mean_theta += particles[i].theta;
    }
    mean_x /= num_particles_;
    mean_y /= num_particles_;
    mean_z /= num_particles_;
    mean_theta /= num_particles_;
    gpsPoint.x = mean_x;
    gpsPoint.y = mean_y;
    gpsPoint.z = mean_z;
    gpsPoint.bearing = mean_theta;
    return true;
}

void ParticleFilter::Print(DbgFrame* dbg_frame)
{
    return dbg_frame->DrawParticles(this);
}